#include "ahrs.h"

#include "../Device/imu.h"
#include "filter.h"
#include "math.h"

#define PI (3.1415926)
float machine_zero_roll = 3.15f;

extern short imu_accel[3], imu_gyro[3];
extern float accel_x, accel_y, accel_z, gyro_x, gyro_y, gyro_z;
extern int16_t compass_data_x, compass_data_y, compass_data_z;
extern float compass_x, compass_y, compass_z;
extern ComplementaryFilter_t MPU6500_Filter_Pitch, MPU6500_Filter_Roll;
extern MedianAverageFilter_t MPU6500_Filter_Yaw;

float Pitch, Roll, Yaw;

void AHRS_Prase_IMU() {
  accel_x = (float)imu_accel[0] / 65520.0f * 16.0f;
  accel_y = (float)imu_accel[1] / 65520.0f * 16.0f;
  accel_z = (float)imu_accel[2] / 65520.0f * 16.0f;

  gyro_x = imu_gyro[0] / 65520.0f * 2000;
  gyro_y = imu_gyro[1] / 65520.0f * 2000;
  gyro_z = imu_gyro[2] / 65520.0f * 2000;
}

void AHRS_Prase_Compass() {
  float compass_tmp_x, compass_tmp_y, compass_tmp_z;
  float sina = sin(Pitch / 180.0f * PI);
  float cosa = cos(Pitch / 180.0f * PI);
  compass_tmp_x = compass_data_x;
  compass_tmp_y = compass_data_y * cosa + compass_data_z * sina;
  compass_tmp_z = compass_data_y * sina - compass_data_z * cosa;
  sina = sin(Roll / 180.0f * PI);
  cosa = cos(Roll / 180.0f * PI);
  compass_x = (compass_tmp_x * cosa + compass_tmp_y * sina);
  compass_y = (-compass_tmp_x * sina + compass_tmp_y * cosa);
  compass_z = compass_tmp_z;
}

void AHRS_AnalyAngle() {
  Pitch = ComplementaryFilter_Apply(&MPU6500_Filter_Pitch,
                                    atan2(accel_x, accel_z) * 180 / PI, gyro_y);
  Roll = ComplementaryFilter_Apply(&MPU6500_Filter_Roll,
                                   atan2(accel_y, accel_z) * 180 / PI, gyro_x) -
         machine_zero_roll;
}

void AHRS_AnalyYaw() {
  Yaw = MedianAverageFilter_Apply(&MPU6500_Filter_Yaw,
                                  atan2(compass_x, compass_y) * 180 / PI);
}
